/*
 * RTSAI - Real-Time Strategy engine for AI research
 * Copyright(C) 2006 DIMAp-UFRN, licensed under LGPL
 *
 * GameObject definitions
 */

#include "GameObject.h"

/*!
 * Test if the object collides with a given unit
 */
bool SphObject :: collideWithSphere ( const Vector3D &c, float r ) const
{
    float dist = Vector3D :: distance ( center, c );
    return ( dist <= radius + r );
}

/*! Checks for intersection of the given spherical obstacle with a
 *  volume of "likely future vehicle positions": a cylinder along the
 *  current path, extending minTimeToCollision seconds along the
 *  forward axis from current position.
 *
 *  If they intersect, a collision is imminent and this function returns
 *  a steering force pointing laterally away from the obstacle's center.
 *
 *  Returns a zero vector if the obstacle is outside the cylinder
 */
Vector3D SphObject :: steerToAvoid (const OpenSteer::AbstractVehicle& v,
                   const float minTimeToCollision) const
{
    if (obstacle)
    {
        // minimum distance to obstacle before avoidance is required
        const float minDistanceToCollision = minTimeToCollision * v.speed();
        const float minDistanceToCenter = minDistanceToCollision + radius;

        // contact distance: sum of radii of obstacle and vehicle
        const float totalRadius = radius + v.radius ();

        // obstacle center relative to vehicle position
        const Vector3D localOffset = center - v.position ();

        // distance along vehicle's forward axis to obstacle's center
        const float forwardComponent = localOffset.dot (v.forward ());
        const Vector3D forwardOffset = forwardComponent * v.forward ();

        // offset from forward axis to obstacle's center
        const Vector3D offForwardOffset = localOffset - forwardOffset;

        // test to see if sphere overlaps with obstacle-free corridor
        const bool inCylinder = offForwardOffset.length() < totalRadius;
        const bool nearby = forwardComponent < minDistanceToCenter;
        const bool inFront = forwardComponent > 0;

        // if all three conditions are met, steer away from sphere center
        if (inCylinder && nearby && inFront)
        {
            return offForwardOffset * -1;
        }
    }
    return Vector3D::zero;  // return no steering force
}

/*!
 *  Test if the object collides with a given unit
 */
bool RectObject :: collideWithSphere ( const Vector3D &c, float r ) const
{
     // TODO write code
     return false;
}


/*! Checks for intersection of the given rectangle obstacle. Not implemented
 *  yet. Returns a zero vector if the obstacle is outside the cylinder
 */

Vector3D RectObject :: steerToAvoid (const OpenSteer::AbstractVehicle& v,
                   const float minTimeToCollision) const
{
    // TODO not implemented yet
    return Vector3D::zero;  // return no steering force
}
